COMMENT ⊗   VALID 00007 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	BEGIN "TEST2"
C00004 00003	α IRON TRIANGLE - CAMERA LOCUS SOLVER
C00005 00004	SUBR LSCAM (ITG I,K,J)
C00006 00005	SUBR CAMINIT
C00009 00006	SUBR TRANSFORM
C00011 00007	α MAIN LOOP
C00012 ENDMK
C⊗;
BEGIN "TEST2"
	REQUIRE "ABBREV[SYS,BGB]" SOURCE_FILE;
	REQUIRE "DPYIII[SYS,BGB]" SOURCE_FILE;
	REQUIRE "SAITRG[SYS,BGB]" SOURCE_FILE;
	SAFE ITG ARRAY DPYBUF[0:999];

α CAMERA;
	REAL PAN,TILT,SWING;
	REAL CX,CY,CZ;
	REAL IX,IY,IZ;
	REAL JX,JY,JZ;
	REAL KX,KY,KZ;
	REAL SX,SY,FOCAL,MAG;	α RASTER SCALES;
	REAL LDX,LDY,PDX,PDY;	α LOGICAL AND PHYSICAL RASTERS;
	REAL XPA,YPA;		α PRINCIPLE AXIS LOCUS IN PP;

α THREE INCH CUBE;

	PRELOAD_WITH 0,3,3,3,0,0;SAFE REAL ARRAY XWC[1:6];
	PRELOAD_WITH 0,0,0,3,3,3;SAFE REAL ARRAY YWC[1:6];
	PRELOAD_WITH 0,0,3,3,3,0;SAFE REAL ARRAY ZWC[1:6];
	PRELOAD_WITH '216,'176,'122,'105,'124,'200;SAFE ITG ARRAY ROW[1:6];
	PRELOAD_WITH '220,'247,'251,'210,'160,'160;SAFE ITG ARRAY COL[1:6];

	SAFE REAL ARRAY XPP,YPP,ZPP,XDC,YDC[1:6];
	SAFE REAL ARRAY XXPP,YYPP,XXDC,YYDC[1:6];
	SAFE REAL ARRAY XCC,YCC,ZCC[1:6];

α IRON TRIANGLE - CAMERA LOCUS SOLVER;

	REAL ARRAY P1,P2,P3,COSANG[1:3],V[1:10,1:3];
	REQUIRE "LS1V3P.REL" LOAD_MODULE;
	EXTERNAL ITG PROCEDURE LS1V3P(REAL ARRAY V,P1,P2,P3,COSANG);

REAL SUBR DOTVEC(ITG I,J);
BEGIN "DOTVEC"
	REAL X1,Y1,Z1,X2,Y2,Z2,R1,R2,ZCOS;
	X1 ← XCC[I]; Y1 ← YCC[I]; Z1 ← ZCC[I];
	X2 ← XCC[J]; Y2 ← YCC[J]; Z2 ← ZCC[J];
	R1 ← SQRT(X1*X1 + Y1*Y1 + Z1*Z1);
	R2 ← SQRT(X2*X2 + Y2*Y2 + Z2*Z2);
	ZCOS←(X1*X2 + Y1*Y2 + Z1*Z2)  /  (R1*R2);
	RETURN(ZCOS);
END "DOTVEC";

SUBR LSCAM (ITG I,K,J);
BEGIN "LSCAM"
	ITG M,N;

α IRON TRIANGLE - KNOWN WORLD LOCI;
	P1[1]←XWC[I]; P2[1]←XWC[J]; P3[1]←XWC[K];
	P1[2]←YWC[I]; P2[2]←YWC[J]; P3[2]←YWC[K];
	P1[3]←ZWC[I]; P2[3]←ZWC[J]; P3[3]←ZWC[K];

α IRON TRIPOD - KNOW ANGLES BETWEEN  CAMERA RAYS;
	COSANG[1] ← DOTVEC(J,K);
	COSANG[2] ← DOTVEC(I,K);
	COSANG[3] ← DOTVEC(I,J);

α THROW THE SHIT AT THE FAN;
	M ← LS1V3P(V,P1,P2,P3,COSANG);
	OUTSTR(9&CVS(M)&" CAMERA SOLUTIONS."&↓);
	FOR N←1 TIL M DO
	OUTSTR(9&CVG(V[N,1])&9&CVG(V[N,2])&9&CVG(V[N,3])&↓);
	INCHRW;
END "LSCAM";
SUBR CAMINIT;
BEGIN "CAMINIT"
	REAL RR;
	REAL C_PAN,S_PAN,C_TILT,S_TILT,C_SWING,S_SWING;

	CX ← -30;	CY ← -24;	CZ ← +22;
	PAN ← -π/4;	TILT ← 60*π/180; MAG ← 32/9;

	FOCAL ← 25;
	PDX ← 10.6;	LDX ← 288;
	PDY ←  8.6;	LDY ← 216;

	SX ← -FOCAL*LDX/PDX;
	SY ← -FOCAL*LDY/PDY;

	C_PAN ← COS(PAN); S_PAN ← SIN(PAN);
	C_TILT ← COS(TILT); S_TILT ← SIN(TILT);
	C_SWING ← COS(SWING); S_SWING ← SIN(SWING);

	IX ←  C_PAN*C_SWING - S_PAN*C_TILT*S_SWING;
	IY ←  S_PAN*C_SWING + C_PAN*C_TILT*S_SWING;
	IZ ←  S_TILT*S_SWING;

	JX ← -C_PAN*S_SWING - S_PAN*C_TILT*C_SWING;
	JY ← -S_PAN*S_SWING + C_PAN*C_TILT*C_SWING;
	JZ ←  S_TILT*C_SWING;

	KX ←  S_PAN*S_TILT;
	KY ← -C_PAN*S_TILT;
	KZ ←       C_TILT;
END "CAMINIT";
SUBR TRANSFORM;
BEGIN "TRANSFORM"
	ITG I;
FOR I←1 TIL 6 DO
BEGIN
	REAL X,Y,Z;
α WC → CC WORLD LOCII;
	X ← XWC[I] - CX;Y ← YWC[I] - CY;Z ← ZWC[I] - CZ;
	XCC[I] ← X*IX + Y*IY + Z*IZ;
	YCC[I] ← X*JX + Y*JY + Z*JZ;
	ZCC[I] ← X*KX + Y*KY + Z*KZ;
α CC → PP;
 	XPP[I] ← SX * XCC[I] / ZCC[I];
	YPP[I] ← SY * YCC[I] / ZCC[I];
α PP → RC;
α	ROW[I] ← 108 - YPP[I]-YPA;
α	COL[I] ← XPP[I] + 144 + XPA;
α RC → PP;
	XPP[I] ← COL[I] - 144 - XPA;
	YPP[I] ← 108 - ROW[I] - YPA;
α PP → CC IMAGE PLANE LOCII;
	XCC[I] ← XPP[I]*FOCAL/SX;
	YCC[I] ← YPP[I]*FOCAL/SY;
	ZCC[I] ← FOCAL;
α PP → DC;
	XDC[I] ← MAG * XPP[I];
	YDC[I] ← MAG * YPP[I];
 	XXDC[I] ← MAG * (COL[I]-144);
 	YYDC[I] ← MAG * (108-ROW[I]);
END;
END "TRANSFORM";

SUBR SHOW;
BEGIN "SHOW"
	ITG I;
	DPYSET(DPYBUF);
	AIVECT(-511,-384);
	AVECT(+511,-384);
	AVECT(+511,+384);
	AVECT(-511,+384);
	AVECT(-511,-384);

	AIVECT(XDC[6],YDC[6]);
	FOR I←1 TIL 6 DO AVECT(XDC[I],YDC[I]);
	AIVECT(XXDC[6],YYDC[6]);
	FOR I←1 TIL 6 DO AVECT(XXDC[I],YYDC[I]);
	DPYOUT(1);
END "SHOW";

α MAIN LOOP;
	CAMINIT;
	TRANSFORM;
	SHOW;
	OUTSTR("*");INCHRW;

	LSCAM(1,2,3);
	LSCAM(1,2,4);
	LSCAM(1,2,5);
	LSCAM(1,2,6);
	LSCAM(2,3,4);

	LSCAM(2,3,5);
	LSCAM(2,3,6);
	LSCAM(3,4,5);
	LSCAM(3,4,6);
	LSCAM(4,5,6);

	WHILE TRUE DO IF INCHRW="X" THEN DONE;
END "TEST2";